#pragma once

#include <cmath>
#include <iostream>
#include <string>
#include <cstdlib>
#include <stack>
#include ".\octatree.h"
#include ".\tricell.h"

using namespace std;

#ifndef MAX_THREAD
	#define MAX_THREAD 4
#endif

bool Triangle_Cube(OctaTree* node, TriCell* tricell);

vector<TriCell*> OctaTree::Intersected_TriCell = { NULL };

OctaTree::OctaTree(double bnd[3][2], int size)
{
	int i,j;

	this->count = 0;
	for(i=0;i<size;i++)
		for(j=0;j<2;j++)
			this->bnd[i][j] = bnd[i][j];
	this->bHasChild = false;
	this->Intersected_TriCell.clear();
}

OctaTree::~OctaTree()
{
	for(int i=0;i<8;i++)
		if(!child[i]) delete child[i];
}

int OctaTree::GetCount()
{
	return this->count;
}

TriCell* OctaTree::GetObj(int no_obj)
{
	int tricell_count = this->count;
	if(no_obj > tricell_count-1 || no_obj<0)
		return NULL;
	else
		return this->obj[no_obj];

}

int OctaTree::Insert(TriCell *objcell)
{
	double Coord[3][3];
	
	int itmp, jtmp;
	bool bIn_nodeA = true;
	bool bIn_nodeB = true;
	bool bIn_nodeC = true;
	bool Crossed = true;

	objcell->GetCoord(Coord,3); 

	bIn_nodeA = bIn_nodeA &&  (Coord[0][0]) <= bnd[0][1] &&  (Coord[0][0]) >=  bnd[0][0];
	bIn_nodeA = bIn_nodeA &&  (Coord[0][1] ) <= bnd[1][1] &&  (Coord[0][1]) >=  bnd[1][0];
	bIn_nodeA = bIn_nodeA &&  (Coord[0][2] ) <= bnd[2][1] &&  (Coord[0][2]) >=  bnd[2][0];

	bIn_nodeB = bIn_nodeB &&  (Coord[1][0] ) <=  bnd[0][1] &&  (Coord[1][0]) >=  bnd[0][0];
	bIn_nodeB = bIn_nodeB &&  (Coord[1][1] ) <=  bnd[1][1] &&  (Coord[1][1]) >=  bnd[1][0];
	bIn_nodeB = bIn_nodeB &&  (Coord[1][2] ) <=  bnd[2][1] &&  (Coord[1][2]) >=  bnd[2][0];

	bIn_nodeC = bIn_nodeC &&  (Coord[2][0] ) <=  bnd[0][1] &&  (Coord[2][0]) >=  bnd[0][0];
	bIn_nodeC = bIn_nodeC &&  (Coord[2][1] ) <=  bnd[1][1] &&  (Coord[2][1]) >=  bnd[1][0];
	bIn_nodeC = bIn_nodeC &&  (Coord[2][2] ) <=  bnd[2][1] &&  (Coord[2][2]) >=  bnd[2][0];

	if(!bIn_nodeA && !bIn_nodeB && !bIn_nodeC)
	{
		Crossed= Triangle_Cube(this, objcell);
	}

	if(bIn_nodeA || bIn_nodeB || bIn_nodeC || Crossed)
	{

		if(!(this->bHasChild))
		{
			if(this->count < MAXOBJ)
			{
				int itmp = this->count;
				this->obj[itmp] = objcell;
				this->count++;
				return 1;
			}
			else
			{
				this->bHasChild = true;
				
				this->CreateChildNode();
				for(int i = 0;i<8;i++)
				{
					(this->child[i])->Insert(objcell);
				}
				for(itmp=MAXOBJ;itmp>0;itmp--)
				{
					for(jtmp=0;jtmp<8;jtmp++)
					{
						(this->child[jtmp])->Insert(this->obj[itmp-1]); 
					}
					this->obj[itmp-1] = NULL;
					this->count--;
				}
				return 1;
			}
		}
		else
		{
			for(int i=0;i<8;i++)
				(this->child[i])->Insert(objcell);
			return 1;
		}
	}
	else
	{
		return 0;
	}
}

void OctaTree::GetBnd(double bnd[][2], int size)
{
	for(int i=0;i<size;i++)
		for(int j=0;j<2;j++)
			bnd[i][j] = this->bnd[i][j];
}

void OctaTree::CreateChildNode(void)
{
	double deltaX, deltaY, deltaZ;
	double childbnd[3][2];
	
	
	deltaX =  this->bnd[0][1] -  this->bnd[0][0];
	deltaY =  this->bnd[1][1] -  this->bnd[1][0];
	deltaZ =  this->bnd[2][1] -  this->bnd[2][0];
	
	childbnd[0][0] =  this->bnd[0][0];
	childbnd[0][1] =  this->bnd[0][0] + deltaX/2.0;
	childbnd[1][0] =  this->bnd[1][0];
	childbnd[1][1] =  this->bnd[1][0] + deltaY/2.0;
	childbnd[2][0] =  this->bnd[2][0];
	childbnd[2][1] =  this->bnd[2][0] + deltaZ/2.0;
	this->child[0] = new OctaTree(childbnd,3); 

	childbnd[0][0] =  this->bnd[0][0];
	childbnd[0][1] =  this->bnd[0][0] + deltaX/2.0;
	childbnd[1][0] =  this->bnd[1][0];
	childbnd[1][1] =  this->bnd[1][0] + deltaY/2.0;
	childbnd[2][0] =  this->bnd[2][0] + deltaZ/2.0;
	childbnd[2][1] =  this->bnd[2][1];
	this->child[1] = new OctaTree(childbnd,3); 
	
	childbnd[0][0] =  this->bnd[0][0];
	childbnd[0][1] =  this->bnd[0][0] + deltaX/2.0;
	childbnd[1][0] =  this->bnd[1][0]+ deltaY/2.0;
	childbnd[1][1] =  this->bnd[1][1] ;
	childbnd[2][0] =  this->bnd[2][0];
	childbnd[2][1] =  this->bnd[2][0] + deltaZ/2.0;
	this->child[2] = new OctaTree(childbnd,3); 

	childbnd[0][0] =  this->bnd[0][0];
	childbnd[0][1] =  this->bnd[0][0] + deltaX/2.0;
	childbnd[1][0] =  this->bnd[1][0] + deltaY/2.0;
	childbnd[1][1] =  this->bnd[1][1];
	childbnd[2][0] =  this->bnd[2][0] + deltaZ/2.0;
	childbnd[2][1] =  this->bnd[2][1];
	this->child[3] = new OctaTree(childbnd,3);
	
	childbnd[0][0] =  this->bnd[0][0] + deltaX/2.0;
	childbnd[0][1] =  this->bnd[0][1];
	childbnd[1][0] =  this->bnd[1][0];
	childbnd[1][1] =  this->bnd[1][0] + deltaY/2.0;
	childbnd[2][0] =  this->bnd[2][0];
	childbnd[2][1] =  this->bnd[2][0] + deltaZ/2.0;
	this->child[4] = new OctaTree(childbnd,3); 

	childbnd[0][0] =  this->bnd[0][0]+ deltaX/2.0;
	childbnd[0][1] =  this->bnd[0][1] ;
	childbnd[1][0] =  this->bnd[1][0];
	childbnd[1][1] =  this->bnd[1][0] + deltaY/2.0;
	childbnd[2][0] =  this->bnd[2][0]+ deltaZ/2.0;
	childbnd[2][1] =  this->bnd[2][1] ;
	this->child[5] = new OctaTree(childbnd,3); 


	childbnd[0][0] =  this->bnd[0][0]+ deltaX/2.0;
	childbnd[0][1] =  this->bnd[0][1] ;
	childbnd[1][0] =  this->bnd[1][0]+ deltaY/2.0;
	childbnd[1][1] =  this->bnd[1][1] ;
	childbnd[2][0] =  this->bnd[2][0];
	childbnd[2][1] =  this->bnd[2][0] + deltaZ/2.0;
	this->child[6] = new OctaTree(childbnd,3); 

	childbnd[0][0] =  this->bnd[0][0]+ deltaX/2.0;
	childbnd[0][1] =  this->bnd[0][1] ;
	childbnd[1][0] =  this->bnd[1][0]+ deltaY/2.0;
	childbnd[1][1] =  this->bnd[1][1] ;
	childbnd[2][0] =  this->bnd[2][0]+ deltaZ/2.0;
	childbnd[2][1] =  this->bnd[2][1] ;
	this->child[7] = new OctaTree(childbnd,3); 
}

void OctaTree::SetInserted(bool ins)
{
	this->bHasChild = ins;
}

bool OctaTree::IsInserted(void)
{
	return this->bHasChild;
}

int OctaTree::RayIntersect(double ray_pos[3], double ray_dir[3], double& ray_len, int rank)
{

	double ray_pos_tmp[3];
	double ray_dir_tmp[3];
	double tx0,tx1,ty0,ty1,tz0,tz1;
	int tri_no = -1;
	unsigned char flagbit;
	double tmin, tmax;

	this->Intersected_TriCell.clear();
	
	flagbit = 0;
	ray_dir_tmp[0] = ray_dir[0];
	ray_pos_tmp[0] = ray_pos[0];
	
	ray_dir_tmp[1] = ray_dir[1];
	ray_pos_tmp[1] = ray_pos[1];

	ray_dir_tmp[2] = ray_dir[2];
	ray_pos_tmp[2] = ray_pos[2];
	
	if(ray_dir[0]<0.0)
	{
		ray_dir_tmp[0] = -ray_dir[0];
		ray_pos_tmp[0] = this->bnd[0][1] + this->bnd[0][0] - ray_pos[0];
		flagbit = flagbit | 4;
	}

	if(ray_dir[1]<0.0)
	{
		ray_dir_tmp[1] = -ray_dir[1];
		ray_pos_tmp[1] = this->bnd[1][1] + this->bnd[1][0] - ray_pos[1];
		flagbit = flagbit | 2;
	}


	if(ray_dir[2]<0.0)
	{
		ray_dir_tmp[2] = -ray_dir[2];
		ray_pos_tmp[2] = this->bnd[2][1] + this->bnd[2][0] - ray_pos[2];
		flagbit = flagbit | 1;
	}
	
	
	tx0 = (this->bnd[0][0] - ray_pos_tmp[0])/ray_dir_tmp[0];
	tx1 = (this->bnd[0][1] - ray_pos_tmp[0])/ray_dir_tmp[0];
	ty0 = (this->bnd[1][0] - ray_pos_tmp[1])/ray_dir_tmp[1];
	ty1 = (this->bnd[1][1] - ray_pos_tmp[1])/ray_dir_tmp[1];
	tz0 = (this->bnd[2][0] - ray_pos_tmp[2])/ray_dir_tmp[2];
	tz1 = (this->bnd[2][1] - ray_pos_tmp[2])/ray_dir_tmp[2];

	tmin = tx0<ty0? ty0: tx0;
	tmin = tmin<tz0? tz0 : tmin;

	tmax = tx1<ty1? tx1: ty1;
	tmax = tmax<tz1? tmax: tz1;

	if((tmin<tmax) && (tmax>0.0))
		proc_subtreeA(tx0,ty0,tz0,tx1,ty1,tz1, this, flagbit, rank);

	for(vector<TriCell*>::iterator cellIt=Intersected_TriCell.begin();cellIt!=Intersected_TriCell.end();cellIt++)
	{
		double t = Triangle_Intersect(*cellIt, ray_pos, ray_dir, true);
		if (fabs(t) <= DBL_EPSILON) continue;
		if(ray_len > t)
		{
			ray_len = t;
			tri_no = (*cellIt)->GetNo();
		}
	}
	return tri_no;
}

int next_Node(double d1, double d2, double d3, int a1, int a2, int a3)
{
	if(d1<=d2)
	{
		if(d1<=d3) 
			return a1;
		else
			return a3;
	}
	else
	{
		if(d2<=d3)
			return a2;
		else
			return a3;
	}
}


void proc_subtreeA(double tx0,double ty0,double tz0,double tx1,double ty1,double tz1, OctaTree *n, unsigned char flagbit, int rank)
{
	unsigned char currNode = 0;
	static double dMinRayLen = INF;

	if((tx1<=0.0) || (ty1<=0.0) || (tz1<=0.0))
		return;

	bool hasChild = n->IsInserted();
	int nodeTriCellCount = n->GetCount();
	if(!hasChild)
	{
		if(nodeTriCellCount>0)
		{
			for(int index=0;index<nodeTriCellCount;index++)
			{
				bool hasNo = false;
				int index2;
				int triCellCount = int(n->Intersected_TriCell.size());
				if(triCellCount >0)
				{
					for(index2 = 0; index2< triCellCount; index2 ++)
					{
						if(n->obj[index] == n->Intersected_TriCell[index2])
						{	
							hasNo = true;
							break;
						}
					}
					if(index2>= triCellCount && !hasNo)
					{
						n->Intersected_TriCell.push_back(n->obj[index]);
					}
				}
				else
				{
					n->Intersected_TriCell.push_back(n->obj[index]);
				}
			}
		}
		return;
	}

	double txM = 0.5*(tx0+tx1);
	double tyM = 0.5*(ty0+ty1);
	double tzM = 0.5*(tz0+tz1);


	if(tx0>ty0)
	{
		if(tx0>tz0)
		{
			if(tx0>tzM) currNode = currNode | 1;
			if(tx0>tyM)	currNode = currNode | 2;
		}
	}
	else if(ty0>tz0)
	{		
		if(ty0>txM) currNode = currNode | 4;
		if(ty0>tzM) currNode = currNode | 1;
	}
	else if(ty0<tz0)
	{
		if(tz0>tyM) currNode = currNode | 2;
		if(tz0>txM)	currNode = currNode | 4;
	}

	do
	{
		switch(currNode)
		{
		case 0:
			proc_subtreeA(tx0,ty0,tz0,txM,tyM,tzM, n->child[flagbit],flagbit, rank);
			currNode = next_Node(txM,tyM,tzM,4,2,1);
			break;
		case 1:
			proc_subtreeA(tx0,ty0,tzM,txM,tyM,tz1,n->child[1^flagbit],flagbit, rank);
			currNode=next_Node(txM,tyM,tz1,5,3,8);
			break;
		case 2:
			proc_subtreeA(tx0,tyM,tz0,txM,ty1,tzM,n->child[2^flagbit],flagbit, rank);
			currNode=next_Node(txM,ty1,tzM,6,8,3);
			break;
		case 3:
			proc_subtreeA(tx0,tyM,tzM,txM,ty1,tz1,n->child[3^flagbit],flagbit, rank);
			currNode=next_Node(txM,ty1,tz1,7,8,8);
			break;
		case 4:
			proc_subtreeA(txM,ty0,tz0,tx1,tyM,tzM,n->child[4^flagbit],flagbit, rank);
			currNode=next_Node(tx1,tyM,tzM,8,6,5);
			break;
		case 5:
			proc_subtreeA(txM,ty0,tzM,tx1,tyM,tz1,n->child[5^flagbit],flagbit, rank);
			currNode=next_Node(tx1,tyM,tz1,8,7,8);
			break;
		case 6:
			proc_subtreeA(txM,tyM,tz0,tx1,ty1,tzM,n->child[6^flagbit],flagbit, rank);
			currNode=next_Node(tx1,ty1,tzM,8,8,7);
			break;
		case 7:
			proc_subtreeA(txM,tyM,tzM,tx1,ty1,tz1,n->child[7^flagbit],flagbit, rank);
			currNode=8;
			break;
		}
	}while(currNode < 8);
	return;
}


bool Triangle_Cube(OctaTree* node, TriCell* tricell)
{
	double rStartPos[3],rEndPos[3],dir[3];
	double CenterPos[6][3];
	double dsn[6][3];
	double t1, tu, rtemp1, rtemp2, rtemp3;;
	double WPos[3];

	double cube_bnd[3][2];
	node->GetBnd(cube_bnd,3); 

	CenterPos[0][0] = CenterPos[2][0] = CenterPos[4][0] = cube_bnd[0][0];
	CenterPos[0][1] = CenterPos[2][1] = CenterPos[4][1] = cube_bnd[1][0];
	CenterPos[0][2] = CenterPos[2][2] = CenterPos[4][2] = cube_bnd[2][0];
	CenterPos[1][0] = CenterPos[3][0] = CenterPos[5][0] = cube_bnd[0][1];
	CenterPos[1][1] = CenterPos[3][1] = CenterPos[5][1] = cube_bnd[1][1];
	CenterPos[1][2] = CenterPos[3][2] = CenterPos[5][2] = cube_bnd[2][1];

	dsn[0][0] = 1.0;
	dsn[0][1] = 0.0;
	dsn[0][2] = 0.0;
	
	dsn[1][0] = -1.0;
	dsn[1][1] = 0.0;
	dsn[1][2] = 0.0;

	dsn[2][0] = 0.0;
	dsn[2][1] = 1.0;
	dsn[2][2] = 0.0;

	dsn[3][0] = 0.0;
	dsn[3][1] = -1.0;
	dsn[3][2] = 0.0;

	dsn[4][0] = 0.0;
	dsn[4][1] = 0.0;
	dsn[4][2] = 1.0;

	dsn[5][0] = 0.0;
	dsn[5][1] = 0.0;
	dsn[5][2] = -1.0;

	double tricell_coord[3][3];
	tricell->GetCoord(tricell_coord,3);

	for(int side=0;side<3;side++)
	{
		for(int axis=0;axis<3;axis++)
		{
			rStartPos[axis] = tricell_coord[side%3][axis];
			rEndPos[axis] = tricell_coord[(side+1)%3][axis];
			dir[axis] = rEndPos[axis] - rStartPos[axis];
		}
		
		t1=0.0;
		tu = INF;
		for(int surf=0;surf<6;surf++)
		{
			if(surf==0)
			{
				WPos[0] = CenterPos[surf][0] - rStartPos[0];
				rtemp1 = dir[0];
				rtemp2 = WPos[0];
			}
			else if(surf==1)
			{
				WPos[0] = CenterPos[surf][0] - rStartPos[0];
				rtemp1 = -dir[0];
				rtemp2 = -WPos[0];
			}
			else if(surf==2)
			{
				WPos[1] = CenterPos[surf][1] - rStartPos[1];
				rtemp1 = dir[1];
				rtemp2 = WPos[1];
			}
			else if(surf==3)
			{
				WPos[1] = CenterPos[surf][1] - rStartPos[1];
				rtemp1 = -dir[1];
				rtemp2 = -WPos[1];
			}
			else if(surf==4)
			{
				WPos[2] = CenterPos[surf][2] - rStartPos[2];
				rtemp1 = dir[2];
				rtemp2 = WPos[2];
			}
			else if(surf==5)
			{
				WPos[2] = CenterPos[surf][2] - rStartPos[2];
				rtemp1 = -dir[2];
				rtemp2 = -WPos[2];
			}
			
			if(abs(rtemp1)>=SMALL)
			{
				rtemp3= rtemp2 / rtemp1;
				if(rtemp1>0.0)
				{
					t1 = t1<rtemp3? rtemp3:t1;
				}
				if(rtemp1<=0.0) 
					tu = (rtemp3<tu)? rtemp3:tu;
			}
			else
			{
				if(rtemp2>0.0) break;
			}
			if(t1>tu) break;
			
		}
		if(t1>0.0 && tu<=1.0 && t1<=tu)
			return true;
	}
	return false;
}